// Prototype
// Version 1.0.0

#include <p32xxxx.h>
#include <plib.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "HelmetSystemConfig.h"
#include "HelmetADC.h"
#include "HelmetTimers.h"
#include "HelmetLEDs.h"
#include "HelmetInputs.h"
#include "HelmetUART.h"
#include "HelmetI2C.h"
#include "HelmetOutputs.h"

/**************************************************************************************
	Main
**************************************************************************************/
main() {
	
	/**************************************
		Configure LEDs
	**************************************/
	ConfigLEDs();
	
	/**************************************
		Turn On Heartbeat LED
	**************************************/
	SetHEARTBEAT();
	
	/**************************************
		Config System
	**************************************/
	ConfigSystem();
	
	/**************************************
		Configure Timer 1
	**************************************/
	ConfigTimer1();
    
    /**************************************
		Configure Atom
	**************************************/	
	ConfigAtom();
	
	/**************************************
		Configure GPS
	**************************************/	
    ConfigGPS();
    
    /**************************************
		Configure Fuel Gauge
	**************************************/
	ConfigFuelGauge();
    
    /**************************************
		Configure ADC
	**************************************/	
	ConfigADC();
	
	/**************************************
		Configure Inputs
	**************************************/
	ConfigInputs();
	
	/**************************************
		Configure Outputs
	**************************************/
	ConfigAllOutputs();
	
	/**************************************
		Enable interrupts
	**************************************/
    EnableInterrupts();
	
	/**************************************
		Startup Message
	**************************************/
	WriteAtom("====================================\r\n");
	
	/**************************************
		Main Loop
	**************************************/
	float accel; // magnitude of g-force experienced
	char lastreadgps; // last GPS character read
	char msg[50]; // generic output message
	float hangtime; // actual hangtime float
	float avgtemp = 0.0; // average temperature
	float avgaccel = 0.0; // average acceleration
	
	while( 1 ) {			
		
		// Blank hud button press
		if( BlankHudButton ) {
			WriteAtom( "Buttn OnOff\r\n" );
			Finished( BlankHudButton );
		}
		
		if( ChangeModeButton ) {
			WriteAtom( "Buttn ChangeMode\r\n" );
			Finished( ChangeModeButton );	
		}	
		
		if( LogDataButton ) {
			WriteAtom( "Buttn LogData\r\n" );
			Finished( LogDataButton );	
		}
		
		// Read the GPS message
		if( ReadGPSMessage ) {
			if( ParseGPSMessage() ) {
				Finished( ReadGPSMessage );
			}
		}
		
		// Interpret the GPS message once read
		if( InterpretGPSMessage ) {
			OutputGPSMessage();
			Finished( InterpretGPSMessage );
		}
		
		// Query GPS for status
		if( GPSQuery ) {
			WriteGPS(">QST<\r");
			WriteGPS(">QTM<\r");
			WriteGPS(">QLN<\r");
			Finished( GPSQuery );
		}	
		
		// Report a gps failure
		if( ReportGPSFailure ) {
			WriteAtom( "Error GPS failure\r\n" );
			Finished( ReportGPSFailure );	
		}	
		
		// Interpret ADC sample data and report acceleration
		if( ConvertADC ) {			
			accel = ConvertADCData();
			sprintf( msg, "Accel %3.1f\r\n", accel );
			WriteAtom( msg );
			Finished( ConvertADC );
		}
		
		// Report Hang Time
		if( ReportHangTime ) {
			hangtime = GetHangTime(); 
			sprintf( msg, "HangT %.2f", hangtime ); 
			WriteAtom( msg );
			Finished( ReportHangTime );
		}
		
		// Report Averages
		if( ReportAverages ) {
			avgaccel = GetAverageAcceleration();
			avgtemp = GetAverageTemperature();
			sprintf( msg, "Avges %.1f %.1f\r\n", avgaccel, avgtemp );
			WriteAtom( msg );
			Finished( ReportAverages );
		}	
		
		// Check Fuel Gauge
		if( CheckFuelGauge >= FUEL_GAUGE_CHECK_INTERVAL ) {
			// status 
			unsigned short int fgstatus = ReadFuelGaugeStatusReg();
			sprintf( msg, "FGstt %d\r\n", fgstatus );
			WriteAtom( msg );
			
			if( (fgstatus&0x06) > 0 ) {
				//WriteAtom("Clearing Status Reg\r\n");
				ClearFuelGaugeStatusReg();
			}	
			
			// battery voltage
			float BattVolt = GetBatteryVoltage();
			sprintf( msg, "FGvlt %.2f V\r\n", BattVolt );
			WriteAtom( msg );
			
			// battery percentage remaining
			unsigned char BattPercent = GetBatteryPercentRemaining();
			sprintf( msg, "FGrel %d %%\r\n", (unsigned char) BattPercent );
			WriteAtom( msg );
			
			// battery charge
			float BattCharge = GetBatteryCharge();
			sprintf( msg, "FGabs %.1f mAh\r\n", BattCharge );
			WriteAtom( msg );
			
			// battery temp
			float BattTemp = GetBatteryTemp();
			sprintf( msg, "FGtmp: %.1f *C\r\n", BattTemp );
			WriteAtom( msg );
			if( BattTemp > 29.0 ) {
				FansOn();
			} else {
				FansOff();
			}	
			
			// battery current
			float BattCurrent = GetBatteryCurrent();
			sprintf( msg, "FGcnt %.4f A\r\n", BattCurrent );
			WriteAtom( msg );
			
			Finished( CheckFuelGauge );
		}		
		
	}
}

